﻿//------------------------------------------------------------------------------
// <copyright file="KinectChooser.cs" company="Microsoft">
//     Copyright (c) Microsoft Corporation.  All rights reserved.
// </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Samples.Kinect.Avateering
{
    using System;
    using System.Diagnostics;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Windows;
    using Microsoft.Kinect;
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;

    /// <summary>
    /// This class will pick a Kinect sensor, if available.
    /// </summary>
    public class KinectChooser : DrawableGameComponent
    {
        /// <summary>
        /// The status to string mapping.
        /// </summary>
        private readonly Dictionary<KinectStatus, string> statusMap = new Dictionary<KinectStatus, string>();

        /// <summary>
        /// The requested color image format.
        /// </summary>
        private ColorImageFormat colorImageFormat ;

        /////// <summary>
        /////// The requested depth image format.
        /////// </summary>
        private DepthImageFormat depthImageFormat;

        /// <summary>
        /// The chooser background texture.
        /// </summary>
        private Texture2D chooserBackground;
        
        /// <summary>
        /// The font for rendering the state text.
        /// </summary>
        private SpriteFont font;

        /// <summary>
        /// Gets or sets near mode.
        /// </summary>
        private bool nearMode;

        /// <summary>
        /// Gets or sets seated mode.
        /// </summary>
        private bool seatedMode;

        ///// <summary>
        ///// Active Kinect sensor
        ///// </summary>
        //private KinectSensor kinectSensor = null;

        /// <summary>
        /// Reader for body frames
        /// </summary>
        private BodyFrameReader reader = null;

        private Microsoft.Kinect.Vector4 floorClipPlane = new Microsoft.Kinect.Vector4();

        private BodyFrame bodyFrame = null;

        /// <summary>
        /// Array for the bodies
        /// </summary>
        private Body[] bodies = null;

        private int counterLooper = 0; 

        /// <summary>
        /// Initializes a new instance of the KinectChooser class.
        /// </summary>
        /// <param name="game">The related game object.</param>
        /// <param name="colorFormat">The desired color image format.</param>
        /// <param name="depthFormat">The desired depth image format.</param>
        public KinectChooser(Game game, ColorImageFormat colorFormat, DepthImageFormat depthFormat)
            : base(game)
        {
            this.colorImageFormat = colorFormat;
            this.depthImageFormat = depthFormat;

            this.nearMode = false;
            this.seatedMode = false;
            
            this.DiscoverSensor();

        }

        /// <summary>
        /// Gets the SpriteBatch from the services.
        /// </summary>
        public SpriteBatch SharedSpriteBatch
        {
            get
            {
                return (SpriteBatch)this.Game.Services.GetService(typeof(SpriteBatch));
            }
        }

        /// <summary>
        /// Gets the selected KinectSensor.
        /// </summary>
        public KinectSensor Sensor { get; private set; }


        /// <summary>
        /// Gets or sets a value indicating whether near mode is enabled.
        /// Near mode enables depth between 0.4 to 3m, default is between 0.8 to 4m.
        /// </summary>
        public bool NearMode
        {
            get
            {
                return this.nearMode;
            }

            set
            {
                //if (null != this.Sensor && null != this.Sensor.DepthStream)
                //{
                //    try
                //    {
                //        this.Sensor.DepthStream.Range = value ? DepthRange.Near : DepthRange.Default;   // set near or default mode
                //        this.nearMode = value;
                //    }
                //    catch (InvalidOperationException)
                //    {
                //        // not valid for this camera
                //    }
                //}
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether seated mode is enabled for skeletal tracking.
        /// Seated mode tracks only the upper body skeleton,
        /// returning the 10 joints of the arms, shoulders and head.
        /// </summary>
        public bool SeatedMode
        {
            get
            {
                return this.seatedMode;
            }

            set
            {
                if (null != this.Sensor && null != this.reader)
                {
                    //try
                    //{
                    //    reader.
                    //    this.Sensor.SkeletonStream.TrackingMode = value ? SkeletonTrackingMode.Seated : SkeletonTrackingMode.Default; // Set seated or default mode
                    //    this.seatedMode = value;
                    //}
                    //catch (InvalidOperationException)
                    //{
                    //    // not valid for this camera
                    //}
                }
            }
        }

        /// <summary>
        /// This method renders the current state of the KinectChooser.
        /// </summary>
        /// <param name="gameTime">The elapsed game time.</param>
        public override void Draw(GameTime gameTime)
        {
            // If the background is not loaded, load it now
            if (null == this.chooserBackground)
            {
                this.LoadContent();
            }

            if (null == this.SharedSpriteBatch)
            {
                return;
            }

            // If we don't have a sensor, or the sensor we have is not connected
            // then we will display the information text
            if (null == this.Sensor || !Sensor.IsOpen)
            {
                this.SharedSpriteBatch.Begin();

                // Render the background
                this.SharedSpriteBatch.Draw(
                    this.chooserBackground,
                    new Vector2(Game.GraphicsDevice.Viewport.Width / 2, Game.GraphicsDevice.Viewport.Height / 2),
                    null,
                    Color.White,
                    0,
                    new Vector2(this.chooserBackground.Width / 2, this.chooserBackground.Height / 2),
                    1,
                    SpriteEffects.None,
                    0);

                // Determine the text
                string txt = KinectStatus.Undefined;
                if (this.Sensor != null)
                {
                    txt = KinectStatus.LastStatus;
                }

                // Render the text
                Vector2 size = this.font.MeasureString(txt);
                this.SharedSpriteBatch.DrawString(
                    this.font,
                    txt,
                    new Vector2((Game.GraphicsDevice.Viewport.Width - size.X) / 2, (Game.GraphicsDevice.Viewport.Height / 2) + size.Y),
                    Color.White);
                this.SharedSpriteBatch.End();
            }

            base.Draw(gameTime);
        }

        /// <summary>
        /// This method loads the textures and fonts.
        /// </summary>
        protected override void LoadContent()
        {
            base.LoadContent();

            this.chooserBackground = Game.Content.Load<Texture2D>("ChooserBackground");
            this.font = Game.Content.Load<SpriteFont>("Segoe16");
        }

        /// <summary>
        /// This method ensures that the KinectSensor is stopped before exiting.
        /// </summary>
        protected override void UnloadContent()
        {
            base.UnloadContent();

            // Always stop the sensor when closing down
            if (this.reader != null)
            {
                // BodyFrameReder is IDisposable
                this.reader.Dispose();
                this.reader = null;
            }

            if (this.Sensor != null)
            {
                this.Sensor.Close();
                this.Sensor = null;
            }
        }

        /// <summary>
        /// This method will use basic logic to try to grab a sensor.
        /// Once a sensor is found, it will start the sensor with the
        /// requested options.
        /// </summary>
        private void DiscoverSensor()
        {
            KinectStatus.LastStatus = KinectStatus.Initializing;
            // Grab any available sensor
            this.Sensor = KinectSensor.GetDefault();
            // open the sensor
            this.Sensor.Open();


            if (null != this.Sensor)
            {
                KinectStatus.LastStatus = "Connected="+this.Sensor.IsAvailable.ToString();

                // If this sensor is connected, then enable it
                if (KinectStatus.LastStatus == "Connected=" + this.Sensor.IsAvailable.ToString())
                {
                    // For many applications we would enable the
                    // automatic joint smoothing, however, in this
                    // Avateering sample, we perform skeleton joint
                    // position corrections, so we will manually
                    // filter when these are complete.

                    // Typical smoothing parameters for the joints:
                    // var parameters = new TransformSmoothParameters
                    // {
                    // Smoothing = 0.25f,
                    // Correction = 0.25f,
                    // Prediction = 0.75f,
                    // JitterRadius = 0.1f,
                    // MaxDeviationRadius = 0.04f 
                    // };

                    try
                    {
                        // get the depth (display) extents
                        //FrameDescription frameDescription = this.Sensor.DepthFrameSource.FrameDescription;
                        //this.displayWidth = frameDescription.Width;
                        //this.displayHeight = frameDescription.Height;
                        openReader();


                        // Enable skeleton tracking in near mode
                        //this.Sensor.SkeletonStream.EnableTrackingInNearRange = true;

                    }
                    catch (Exception exp)
                    {
                        Debug.WriteLine("exp=" + exp);
                        KinectStatus.LastStatus = KinectStatus.Error + exp;
                    }
                }
            }
            else
            {
                KinectStatus.LastStatus = KinectStatus.Disconnected;
            }
        }

        public void openReader()
        {
              // open the reader for the body frames
              if (null != this.Sensor && this.Sensor.IsOpen)
              {
                  this.reader = this.Sensor.BodyFrameSource.OpenReader();
                  this.reader.FrameArrived += this.Reader_FrameArrived;
              }
        }
        public float elevationAngle()
        {   
            double angle = Math.Atan(this.floorClipPlane.Z / this.floorClipPlane.Y) * 180 / Math.PI;
            return (float)angle;
        }

        public Microsoft.Kinect.Vector4 getFloorClipPlane()
        {
            return this.floorClipPlane;
        }

        void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            counterLooper++;
            
            using (var reference = e.FrameReference.AcquireFrame() )
            {

                if (null == reference)
                {
                    Debug.WriteLineIf(this.counterLooper%30 == 0 , "Reader_FrameArrived NULL e.FrameReference=" + e.FrameReference.ToString());
                }
                else
                {
                    bodyFrame = reference;
                    this.bodies = new Body[reference.BodyCount];
                    reference.GetAndRefreshBodyData(this.bodies);
                    this.floorClipPlane = reference.FloorClipPlane;
                }

            }
        }

        public BodyFrame getBodyFrame()
        {
            return bodyFrame;
        }

        public Body[] letTheBodiesHitTheFloor()
        {
            return bodies; 
        }

        public Body getReaderActiveBody()
        {
            try
            {
                if (null == this.bodies)
                    return null;
                if (this.bodies.Length >0)
                {
                foreach (Body body in this.bodies)
                    {
                        if (null !=body && body.IsTracked)
                        {
                            return body;
                        }
                    }
                }
            }
            catch (Exception eee) {
                Debug.WriteLine("exception in body aquire" + eee);
            }

            return null;
        }

        //private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        //{
        //    BodyFrameReference frameReference = e.FrameReference;
        //    try
        //    {
        //        BodyFrame frame = frameReference.AcquireFrame();

        //        if (frame != null)
        //        {
        //            // BodyFrame is IDisposable
        //            using (frame)
        //            {
        //               if (this.bodies == null)
        //                    {
        //                        this.bodies = new Body[frame.BodyCount];
        //                    } 
                        
        //                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
        //                    // As long as those body objects are not disposed and not set to null in the array,
        //                    // those body objects will be re-used.
        //                    frame.GetAndRefreshBodyData(this.bodies);
        //                    // initialize datagrid for bodies 
        //                    //displayBodies.ItemsSource = bodies;
        //                    int skeletonNumber = 1;
        //                    foreach (Body body in this.bodies)
        //                    {
        //                        if (body.IsTracked)
        //                        {
        //                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
        //                            // convert the joint points to depth (display) space
        //                            Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
        //                            foreach (JointType jointType in joints.Keys)
        //                            {
        //                                DepthSpacePoint depthSpacePoint = this.kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(joints[jointType].Position);
        //                                jointPoints[jointType] = new System.Windows.Point(depthSpacePoint.X, depthSpacePoint.Y);
        //                            }

        //                            this.DrawBody(joints, jointPoints);

        //                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
        //                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
        //                            var head = body.Joints[JointType.Head];
                                    
        //                            skeletonNumber++;
        //                        }
        //                    }
        //              }
        //          }
        //    }
        //    catch (Exception e)
        //    {
        //        // ignore if the frame is no longer available
        //        Debug.WriteLine("error " + e);
        //    }
        //}


    }
}
